#include "discrete_field_system.h"

namespace Field_packing
{
	Discrete_field_system::Discrete_field_system(const std::vector<Packing_polygon*>& polygon_set, const Container_box& bndbox, Sync_queue< std::vector<Parameter> >& q)
		: polygons(polygon_set), System_potential_energy(bndbox), dfg(new Distance_field_grid_2(bndbox, 200, 200)), trans_queue(q)//, ofs("points.txt")
	{
		//System_potential_energy::epsilon = 10;
		//dfg = new Distance_field_grid_2(bndbox, 100, 100);
		for (size_t i = 0; i < polygon_set.size(); i++)
			dfg->append_distance_field(*polygon_set[i]);
		sample_pnts.resize(polygon_set.size());

		std::vector< std::vector<double> > edge_lens(polygon_set.size());
		std::valarray<double> perimeters(0.0, polygon_set.size());
		std::valarray<double> areas(polygon_set.size());
		for (size_t i = 0; i < polygon_set.size(); i++)
		{
			edge_lens[i].reserve(polygon_set[i]->size());
			for (size_t j = 0; j < polygon_set[i]->size(); j++)
			{
				edge_lens[i].push_back(CGAL::sqrt(polygon_set[i]->edge(j).squared_length()));
				perimeters[i] += edge_lens[i].back();
			}
			areas[i] = polygon_set[i]->volume();
		}

		const unsigned int sample_nb = 75; // how many points on each polygon
		rm = perimeters.sum();
		rm /= polygon_set.size();
		rm /= 8*sample_nb;
		//std::cout<<"r_m = "<<rm<<std::endl;
		
		// sample on the edges of polygons
		//for (size_t i = 0; i < polygon_set.size(); i++)
		//{
		//	sample_pnts[i].reserve(sample_nb);
		//	for (size_t j = 0; j < polygon_set[i]->size(); j++)
		//	{
		//		unsigned int N = edge_lens[i][j]/perimeters[i]*sample_nb + 0.5;
		//		N = std::max<unsigned int>(N, 1);
		//		Point_2 sp = polygon_set[i]->edge(j).source();
		//		Point_2 tp = polygon_set[i]->edge(j).target();
		//		for (size_t k = 0; k < N; k++)
		//		{
		//			Point_2 p(((N-k)*sp.x()+k*tp.x())/N, ((N-k)*sp.y()+k*tp.y())/N);
		//			sample_pnts[i].push_back(p);
		//		}
		//	}
		//}
		// sample in the interior of polygons
		double max_area = areas.max();
		for (size_t i = 0; i < polygon_set.size(); i++)
		{
			Polygon_sampler ps(*polygon_set[i]);
			ps.inner_sample(areas[i]/max_area*sample_nb, sample_pnts[i]);
		}
		// identity matrix at the beginning
		prev_trans.reserve(polygon_set.size());
		for (size_t i = 0; i < polygon_set.size(); i++)
		{
			prev_trans.push_back(matrix<double>(3,3));
			matrix<double>& m = prev_trans.back();
			m(0,0) = m(1,1) = m(2,2) = 1.0;
			m(0,1) = m(1,0) = m(0,2) = m(2,0) = m(1,2) = m(2,1) = 0.0;
		}

		// set up the optimization context
		int nb_constraints = 0;
#if 1
		for (size_t i = 0; i < polygons.size(); i++)
			nb_constraints += polygons[i]->size();
		nb_constraints *= 2; // constraint on the x and y coordinate of vertices
#endif
		ktr_context = std::auto_ptr<Knitro_context>(new Knitro_context(polygons.size()*3, nb_constraints, nb_constraints*3, 0));
		ktr_context->objType = KTR_OBJTYPE_GENERAL;
		ktr_context->objGoal = KTR_OBJGOAL_MINIMIZE;
		for (size_t i = 0; i < polygons.size(); i++)
		{
			ktr_context->xLoBnds[3*i] = -KTR_INFBOUND;
			ktr_context->xUpBnds[3*i] = KTR_INFBOUND;
			ktr_context->xLoBnds[3*i+1] = ktr_context->xLoBnds[3*i+2] = -KTR_INFBOUND;
			ktr_context->xUpBnds[3*i+1] = ktr_context->xUpBnds[3*i+2] = KTR_INFBOUND;
		}
		//*(ktr_context->cLoBnds) = -1;
		//*(ktr_context->cUpBnds) = 1;
		//*(ktr_context->cType) = KTR_CONTYPE_LINEAR;
		//*(ktr_context->jacIndexCons) = *(ktr_context->jacIndexVars) = 0;
#if 1
		int *ptrCType = ktr_context->cType;
		double *ptrCLoBnds = ktr_context->cLoBnds, *ptrCUpBnds = ktr_context->cUpBnds;
		for (size_t i = 0; i < polygons.size(); i++)
		{
			for (size_t j = 0; j < polygons[i]->size(); j++)
			{
				*ptrCType++ = KTR_CONTYPE_GENERAL;
				*ptrCLoBnds++ = bounding_box.x_min();
				*ptrCUpBnds++ = bounding_box.x_max();
				*ptrCType++ = KTR_CONTYPE_GENERAL;
				*ptrCLoBnds++ = bounding_box.y_min();
				*ptrCUpBnds++ = bounding_box.y_max();
			}
		}
		int constraint_no = 0;
		int *ptrCons = ktr_context->jacIndexCons, *ptrVars = ktr_context->jacIndexVars;
		for (size_t i = 0; i < polygons.size(); i++)
		{
			for (size_t j = 0; j < polygons[i]->size(); j++)
			{
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i;
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i+1;
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i+2;
				constraint_no++;
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i;
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i+1;
				*ptrCons++ = constraint_no;
				*ptrVars++ = 3*i+2;
				constraint_no++;
			}
		}
#endif
		//ofs << "iteration points:\n";
	}

	//Discrete_field_system::~Discrete_field_system()
	//{
	//	for (size_t i = 0; i < polygons.size(); i++)
	//	{
	//		matrix<double>& m = prev_trans[i];
	//		Transformation_2 back(	m(0,0), m(1,0), -(m(0,0)*m(0,2)+m(1,0)*m(1,2)),
	//								m(0,1), m(1,1), -(m(0,1)*m(0,2)+m(1,1)*m(1,2)));
	//		*(polygons[i]) *= back;
	//	}
	//}
	double Discrete_field_system::molecular_energy_at(const double *const x)
	{
		double LJ_energy = 0.0;
		move_to_new_point(x);
		for (size_t i = 0; i < polygons.size(); i++) // for every polygon
		{
			for (size_t j = 0; j < polygons.size(); j++) // for every sample point on each polygon
			{
				if (i == j) continue;
				for (size_t k = 0; k < sample_pnts[i].size(); k++) // compute potential energy with the other polygons pairwise
				{
					double d = dfg->distance_query(sample_pnts[i][k], j);

					if (d <= 0)	d = rm/50;

					double rm6_over_d6 = std::pow(rm/d, 2/*6*/);
					LJ_energy += rm6_over_d6*(rm6_over_d6 - 2);
				}
			}
		}
		return LJ_energy*epsilon;
	}
	void Discrete_field_system::molecular_energy_gradient_at(const double *const x, double *gradient)
	{
		move_to_new_point(x);
		for (size_t i = 0; i < polygons.size(); i++)
		{
			matrix<double>& mi = prev_trans[i];
			Transformation_2 backi(	mi(0,0), mi(1,0), -(mi(0,0)*mi(0,2)+mi(1,0)*mi(1,2)),
									mi(0,1), mi(1,1), -(mi(0,1)*mi(0,2)+mi(1,1)*mi(1,2)));
			std::vector<Point_2> original_pnts(sample_pnts[i].size());
			std::transform(sample_pnts[i].begin(), sample_pnts[i].end(), original_pnts.begin(), backi);
			double si = std::sin(x[3*i]), ci = std::cos(x[3*i]);
			for (size_t j = 0; j < polygons.size(); j++)
			{
				if (i==j) continue;
				double sj = std::sin(x[3*j]), cj = std::cos(x[3*j]);
				matrix<double>& mj = prev_trans[j];
				Transformation_2 backj(	mj(0,0), mj(1,0), -(mj(0,0)*mj(0,2)+mj(1,0)*mj(1,2)),
										mj(0,1), mj(1,1), -(mj(0,1)*mj(0,2)+mj(1,1)*mj(1,2)));				
				for (size_t k = 0; k < sample_pnts[i].size(); k++)
				{
					Vector_2 g;
					Point_2 closest_pnt;
					double d = dfg->gradient_query(sample_pnts[i][k], j, g, closest_pnt);
					double common_factor;

					if (d <= 0)	d = rm/50;

					double rm6_over_d6 = std::pow(rm/d, 2/*6*/);
					common_factor = -/*6*/2*rm6_over_d6*(rm6_over_d6 - 1);
					common_factor /= d*d;
					common_factor *= epsilon;

					g = Vector_2(closest_pnt, sample_pnts[i][k]);
					g = 2*g;
					const Point_2& p = original_pnts[k];
					closest_pnt = backj(closest_pnt);
					gradient[3*i] += g*Vector_2(-p.x()*si-p.y()*ci, p.x()*ci-p.y()*si)*common_factor;
					gradient[3*i+1] += g.x()*common_factor;
					gradient[3*i+2] += g.y()*common_factor;
					gradient[3*j] += -g*Vector_2(-closest_pnt.x()*sj-closest_pnt.y()*cj, closest_pnt.x()*cj-closest_pnt.y()*sj)*common_factor;
					gradient[3*j+1] += -g.x()*common_factor;
					gradient[3*j+2] += -g.y()*common_factor;
				}
			}
		}
	}
	void Discrete_field_system::energy_gradient_at(const double *const x, double *gradient)
	{
		std::fill(gradient, gradient + ktr_context->n, 0.0);
		molecular_energy_gradient_at(x, gradient);
	}
	double Discrete_field_system::system_energy_at(const double *const x)
	{
		return molecular_energy_at(x);
	}

	void Discrete_field_system::constraint_at(const double *const x, double *constraint_values)
	{
		for (size_t i = 0; i < polygons.size(); i++)
		{
			double s = std::sin(x[3*i]), c = std::cos(x[3*i]);
			Transformation_2 t(c, -s, x[3*i+1], s, c, x[3*i+2]);
			for (size_t j = 0; j < polygons[i]->size(); j++)
			{
				Point_2 p = t(polygons[i]->vertex(j));
				*constraint_values++ = p.x();
				*constraint_values++ = p.y();
			}
		}
	}
	void Discrete_field_system::constraint_grad_at(const double *const x, double *constraint_grad)
	{
		for (size_t i = 0; i < polygons.size(); i++)
		{
			double s = std::sin(x[3*i]), c = std::cos(x[3*i]);
			//Transformation_2 t(c, -s, x[3*i+1], s, c, x[3*i+2]);
			for (size_t j = 0; j < polygons[i]->size(); j++)
			{
				Point_2 p = polygons[i]->vertex(j);
				*constraint_grad++ = -p.x()*s - p.y()*c;
				*constraint_grad++ = 1.0;
				*constraint_grad++ = 0.0;
				*constraint_grad++ = p.x()*c-p.y()*s;
				*constraint_grad++ = 0.0;
 				*constraint_grad++ = 1.0;
			}
		}
	}

	void Discrete_field_system::move_to_new_point(const double *const x)
	{
		for (size_t i = 0; i < polygons.size(); i++)
		{
			// update the field
			double s = std::sin(x[3*i]), c = std::cos(x[3*i]);
			matrix<double>& m = prev_trans[i];
			Transformation_2 back(	m(0,0), m(1,0), -(m(0,0)*m(0,2)+m(1,0)*m(1,2)),
									m(0,1), m(1,1), -(m(0,1)*m(0,2)+m(1,1)*m(1,2)));
			Transformation_2 t = Transformation_2(c, -s, x[3*i+1], s, c, x[3*i+2])*back;
			dfg->update_distance_field(i, t);
			// update the sampling points
			for (size_t j = 0; j < sample_pnts[i].size(); j++)
				sample_pnts[i][j] = t(sample_pnts[i][j]);
			// record the most recent transformation
			m(0,0) = c;
			m(0,1) = -s;
			m(0,2) = x[3*i+1];
			m(1,0) = s;
			m(1,1) = c;
			m(1,2) = x[3*i+2];
		}	
	}
	int Discrete_field_system::optimize(double *trans, bool apply_constraints)
	{
		KTR_context* kc = ktr_context->kc;
		if (kc == NULL)
			return -1000 ; // probably a license issue
		if (KTR_set_int_param_by_name(kc, "algorithm", 1) != 0)
			return -1000;
		/* set options: automatic gradient and hessian matrix */
		if (KTR_set_int_param_by_name(kc, "bar_murule", 2) != 0) // "2" is plausible up to now if the above algo para is "1". "6" a bit less plausible
			return -1000;
		if (KTR_set_int_param_by_name(kc, "gradopt", KTR_GRADOPT_EXACT) != 0)
			return -1000 ;
		if (KTR_set_int_param_by_name(kc, "hessopt", KTR_HESSOPT_BFGS) != 0)
			return -1000 ;
		//if (KTR_set_int_param_by_name(kc, "bar_directinterval", 0) != 0)
		//	return -1000;
		if (KTR_set_int_param_by_name(kc, "bar_feasible", 1) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "maxcgit", 0) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "xtol", 1e-20) != 0)
			return -1000;
		// debug parameters
		//if (KTR_set_int_param_by_name(kc, "debug", 1) != 0)
		//	return -1000;
		//if (KTR_set_int_param_by_name(kc, "outlev", 3) != 0)
		//	return -1000 ;
		//if (KTR_set_int_param_by_name(kc, "outmode", 0) != 0)
		//	return -1000;
		if (KTR_set_int_param_by_name(kc, "newpoint", 3) != 0)
			return -1000;
		if (KTR_set_int_param_by_name(kc, "bar_maxbacktrack", 10) != 0)
			return -1000;
		//if (KTR_set_int_param_by_name(kc, "bar_directinterval", 5) != 0)
		//	return -1000;
		if (KTR_set_int_param_by_name(kc, "maxit", 0) != 0)
			return -1000 ;
		if (KTR_set_double_param_by_name(kc, "opttol", 1e-9) != 0)
			return -1000;
		//if (KTR_set_int_param_by_name(kc, "ms_enable", 1) != 0)
		//	return -1000;
		//if (KTR_set_int_param_by_name(kc, "ms_maxsolves", 5) != 0)
		//	return -1000;
		//if (KTR_set_int_param_by_name(kc, "ms_maxbndrange", 1) != 0)
		//	return -1000;
		/* register the callback function */
		if (KTR_set_func_callback(kc, &callback) != 0)
			return -1000 ;
		if (KTR_set_grad_callback(kc, &callback) != 0)
			return -1000 ;
		if (KTR_set_newpoint_callback(kc, &newpoint_callback) != 0)
			return -1000;

		int nStatus = KTR_init_problem(	kc, ktr_context->n, ktr_context->objGoal, ktr_context->objType, 
										ktr_context->xLoBnds, ktr_context->xUpBnds, ktr_context->m,
										ktr_context->cType, ktr_context->cLoBnds, ktr_context->cUpBnds,
										ktr_context->nnzJ, ktr_context->jacIndexVars, ktr_context->jacIndexCons,
										ktr_context->nnzH, ktr_context->hessIndexRows, ktr_context->hessIndexCols, ktr_context->xInitials, NULL);
		if (nStatus)	return nStatus;
		nStatus = KTR_solve(kc, trans, ktr_context->lambda, 0, &ktr_context->obj, NULL, NULL, NULL, NULL, NULL, this);

		/* direvative check */
		//double init_obj = system_energy_at(trans);
		//std::fill(trans, trans + ktr_context->n, 0.0);
		//nStatus = KTR_check_first_ders(kc, trans, 1, 1.0e-10, 1.0e-10, 0, init_obj, NULL, NULL, NULL, this);
		/*************/

		return nStatus;
	}

	int Discrete_field_system::steepest_descent_optimize(double *trans)
	{

		double alpha = 0.001, beta = 0.5;
		double t = 1.0;
		// initialize as no motion. this should be always feasible
		std::fill(trans, trans + ktr_context->n, 0.0);
		double *descent_dir = new double[ktr_context->n];
		energy_gradient_at(trans, descent_dir);
		//std::cout<<'[';
		for (size_t i = 0; i < ktr_context->n; i++)
		{
			descent_dir[i] = -descent_dir[i];
			//std::cout<<descent_dir[i]<<' ';
		}
		//std::cout<<']'<<std::endl;
		double new_point_val = 0.0, val_at_init = molecular_energy_at(trans), line_val = -std::numeric_limits<double>::max();
		int inc = 1;
		double dir_norm2 = DDOT(&ktr_context->n, descent_dir, &inc, descent_dir, &inc);
		size_t max_iter = 100, iter = 0;
		while (new_point_val > line_val && iter < max_iter)
		{
			//pre_point_val = molecular_energy_at(trans);
			//line_val = pre_point_val - alpha*t*dir_norm2;
			// go to the new point
			//for (size_t i = 0; i < ktr_context->n; i++)	
			//	trans[i] = trans[i] + t*descent_dir[i];
			std::fill(trans, trans + ktr_context->n, 0.0);
			line_val = val_at_init - alpha*t*dir_norm2;
			DAXPY(&ktr_context->n, &t, descent_dir, &inc, trans, &inc);
			new_point_val = molecular_energy_at(trans);
			t *= beta;
			iter++;
			//std::cout<<"cur val = "<<new_point_val<<", init val = "<<val_at_init<<", line val = "<<line_val<<std::endl;
		}
		//std::cout<<", t = "<<t<<std::endl;
		/*
		** ONLY FOR DEBUG
		*/
		//std::ofstream step_val_file("step_val_fl.txt");
		//double *db_x = new double[ktr_context->n];
		//for (size_t i = 0; i < 100; i++)
		//{
		//	double t = i/100.0;
		//	std::fill(db_x, db_x + ktr_context->n, 0.0);
		//	DAXPY(&ktr_context->n, &t, descent_dir, &inc, db_x, &inc);
		//	double energy_val = molecular_energy_at(db_x);
		//	step_val_file<<t<<' '<<energy_val<<';';
		//}
		//delete[] db_x;
		delete[] descent_dir;
		if (iter == max_iter)	
			return -401;
		else
			return 0;
	}

	int Discrete_field_system::directional_descent_optimize(double *trans)
	{
		int max_iter = 1000;
		int iter_no = 0;
		double angle_base = boost::math::constants::pi<double>()/90;
		std::vector<double> trans_base(polygons.size());
		for (size_t i = 0; i < polygons.size(); i++)
			trans_base[i] = std::sqrt(polygons[i]->volume());
		std::fill(trans, trans + ktr_context->n, 0.0);
		double *grad = new double[ktr_context->n];
		//for (size_t i = 0; i < ktr_context->n; i++)
		//	descent_dir[i] = -descent_dir[i];
		for (iter_no = 0; iter_no < max_iter; iter_no++)
		{
			energy_gradient_at(trans, grad);
			for (size_t i = 0; i < polygons.size(); i++)
			{
				double decay_factor = std::pow(0.5, iter_no/(max_iter-iter_no));
				double angle_step = angle_base*decay_factor;
				//std::cout<<- angle_step*(grad[3*i]/std::fabs(grad[3*i]))<<',';
				trans[3*i] = trans[3*i] - angle_step*(grad[3*i]/std::fabs(grad[3*i]));
				Vector_2 tv(grad[3*i+1], grad[3*i+2]);
				tv = tv/std::sqrt(tv.squared_length());
				double trans_step = trans_base[i]*decay_factor;
				//std::cout<<- trans_step*tv.x()<<',';
				trans[3*i+1] = trans[3*i+1] - trans_step*tv.x();
				//std::cout<<- trans_step*tv.y()<<std::endl;
				trans[3*i+2] = trans[3*i+2] - trans_step*tv.y();
			}
			//double e = system_energy_at(trans);
			//std::cout<<"System Energy = "<<e<<std::endl;
		}
		delete[] grad;
		return 0; // should always succeed
	}
	int Discrete_field_system::newpoint_callback(const int evalRequestCode, const int n, const int m, const int nnzJ, 
							const int nnzH,	const double * const x,	const double * const lambda, 
							double * const obj, double * const c, double * const objGrad, double * const jac,
							double * const hessian,	double * const hessVector, void * userParams)
	{
		Discrete_field_system *pthis = static_cast< Discrete_field_system* >(userParams);
		// transform back
		std::vector<Parameter> paras;
		paras.reserve(pthis->polygons.size());
		for (size_t i = 0; i < pthis->polygons.size(); i++)
		{
			paras.push_back(Parameter(1.0, x[3*i], x[3*i+1], x[3*i+2]));
		}
		pthis->trans_queue.push(paras);
		
		return 0;
	}

	void Discrete_field_system::draw_sample_pnts() const
	{
		glColor3f(1.0f, 0.0f, 0.0f);
		glPointSize(2.0f);
		glBegin(GL_POINTS);
		for (size_t i = 0; i < sample_pnts.size(); i++)
		{
			for (size_t j = 0; j < sample_pnts[i].size(); j++)
			{
				glVertex2d(sample_pnts[i][j].x(), sample_pnts[i][j].y());
			}
		}
		glEnd();
	}

	void Discrete_field_system::plot_graph()
	{
		std::ofstream offset_energy_file("offset_energy.txt");
		//double stepx = ktr_context->cUpBnds[0] - ktr_context->cLoBnds[0];
		double stepx = 30.0;
		stepx /= 100;
		double stepy = ktr_context->cUpBnds[1] - ktr_context->cLoBnds[1];
		stepy /= 100;
		double *trans = new double[ktr_context->n];
		std::fill(trans, trans + ktr_context->n, 0.0);
		//offset_energy_file<<'[';
		for (size_t i = 0; i < 100; i++)
		{
			trans[0] = i*stepx;
			for (size_t j = 0; j < 100; j++)
			{
				trans[2] = j*stepy;
				double energy = molecular_energy_at(trans);
				offset_energy_file << trans[0] << ' ' << trans[2] << ' ' << energy << std::endl;
			}
		}
		//offset_energy_file << ']' << std::endl;
		delete[] trans;
	}
}